/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
// Victor 1 & 2 - Driving Left
// Victor 3 & 4 - Driving Right
// Encoders - 3, 4, 5, 6
// Gyro I2C
// Spike 1 - Compressor
// Spike 2 - Light
// Spike
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Solenoid;
import java.util.Timer;
import java.util.TimerTask;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Howie extends IterativeRobot {

    DemobotPneumaticSystem PneumaticSystem = DemobotPneumaticSystem.getInstance();
    double curve = 0.3;
    // Victors for controlling drive train
    Victor leftVictor1 = new Victor(1);
    Victor leftVictor2 = new Victor(2);
    Victor rightVictor1 = new Victor(3);
    Victor rightVictor2 = new Victor(4);
    Autonomous autonomous = new Autonomous();
    private String currentRoutine = "forward";
    Controller MyController = Controller.getInstance();
    public String currentEffecer = "animatronic";

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        Timer dashTimer = new Timer();

        dashTimer.schedule(new TimerTask() {

            public void run() {
                updateSmartDashboard();
            }
        }, 0, 200);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        autonomous.runRoutine(currentRoutine);
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        MyController.update();
        updateDrive();
        if (currentEffecer.equals("animatronic")) {
            PneumaticSystem.updateAnimatronic();
        }
        if (currentEffecer.equals("t-shirt cannon")) {
            PneumaticSystem.updateTShirtCannon();
        }
        updateSmartDashboard();
    }

    public void updateDrive() {
        double leftSpeed = curve * MyController.getLeftY();
        double rightSpeed = curve * MyController.getRightY();
        setDrive(leftSpeed, rightSpeed);
    }

    // multiplies inputs to control four victors, setting them in groups of two
    // right victor values are negated to account for the reversal in the gear system
    public void setDrive(double leftSpeed, double rightSpeed) {
        leftVictor1.set(leftSpeed);
        leftVictor2.set(leftSpeed);
        rightVictor1.set(-rightSpeed);
        rightVictor2.set(-rightSpeed);
    }
//for the animatronic
//checks for the 2 and 3 buttons to raise and lower neck

    // keeps the compressor on unless it is full, if activated by the gigaware extends the animatronic neck upwards
   

    // TODO figure out how to make SmartDashboard Data actually visible
    private void updateSmartDashboard() {
        SmartDashboard.putDouble("Left Y", MyController.getLeftY());
        SmartDashboard.putDouble("Right Y", MyController.getRightY());
        SmartDashboard.putString("currentState", MyController.currentState.ToString());
    }
}
